// src/quat.ts

import { ReadonlyQuat, Quat, ReadonlyVec3, Vec3, ReadonlyMat3, Mat3 } from './types';

const EPSILON = 1e-6;

/**
 * 创建一个新的单位四元数。
 * @returns 返回一个表示单位四元数的数组 `[0, 0, 0, 1]`。
 */
export function create(): Quat {
  return [0, 0, 0, 1];
}

/**
 * 将四元数设置为单位四元数。
 * @param out - 输出的四元数。
 * @returns 返回设置为 `[0, 0, 0, 1]` 的单位四元数。
 */
export function identity(out: Quat): Quat {
  out[0] = 0;
  out[1] = 0;
  out[2] = 0;
  out[3] = 1;
  return out;
}

/**
 * 根据给定的角度和旋转轴设置四元数。
 * @param out - 输出的四元数。
 * @param axis - 旋转轴向量 `[x, y, z]`。
 * @param rad - 旋转角度（弧度制）。
 * @returns 返回根据轴和角度计算得到的四元数。
 */
export function setAxisAngle(out: Quat, axis: ReadonlyVec3, rad: number): Quat {
  const halfRad = rad * 0.5;
  const s = Math.sin(halfRad);
  out[0] = s * axis[0];
  out[1] = s * axis[1];
  out[2] = s * axis[2];
  out[3] = Math.cos(halfRad);
  return out;
}

/**
 * 获取给定四元数的旋转轴和角度。
 * @param out_axis - 输出的旋转轴向量 `[x, y, z]`。
 * @param q - 输入的四元数。
 * @returns 返回旋转角度（弧度制）。
 */
export function getAxisAngle(out_axis: Vec3, q: ReadonlyQuat): number {
  const rad = Math.acos(q[3]) * 2.0;
  const s = Math.sin(rad / 2.0);
  if (s > EPSILON) {
    out_axis[0] = q[0] / s;
    out_axis[1] = q[1] / s;
    out_axis[2] = q[2] / s;
  } else {
    out_axis[0] = 1;
    out_axis[1] = 0;
    out_axis[2] = 0;
  }
  return rad;
}

/**
 * 获取两个单位四元数之间的角度距离。
 * @param a - 第一个四元数。
 * @param b - 第二个四元数。
 * @returns 返回两个四元数之间的角度距离（弧度制）。
 */
export function getAngle(a: ReadonlyQuat, b: ReadonlyQuat): number {
  const dotproduct = dot(a, b);
  return Math.acos(2 * dotproduct * dotproduct - 1);
}

/**
 * 乘以两个四元数。
 * @param out - 输出的四元数。
 * @param a - 第一个四元数。
 * @param b - 第二个四元数。
 * @returns 返回两个四元数相乘的结果。
 */
export function multiply(out: Quat, a: ReadonlyQuat, b: ReadonlyQuat): Quat {
  const ax = a[0], ay = a[1], az = a[2], aw = a[3];
  const bx = b[0], by = b[1], bz = b[2], bw = b[3];

  out[0] = ax * bw + aw * bx + ay * bz - az * by;
  out[1] = ay * bw + aw * by + az * bx - ax * bz;
  out[2] = az * bw + aw * bz + ax * by - ay * bx;
  out[3] = aw * bw - ax * bx - ay * by - az * bz;
  return out;
}

/**
 * 绕 X 轴旋转四元数。
 * @param out - 输出的四元数。
 * @param a - 输入的四元数。
 * @param rad - 旋转角度（弧度制）。
 * @returns 返回绕 X 轴旋转后的四元数。
 */
export function rotateX(out: Quat, a: ReadonlyQuat, rad: number): Quat {
  const halfRad = rad * 0.5;
  const ax = a[0], ay = a[1], az = a[2], aw = a[3];
  const bx = Math.sin(halfRad), bw = Math.cos(halfRad);

  out[0] = ax * bw + aw * bx;
  out[1] = ay * bw + az * bx;
  out[2] = az * bw - ay * bx;
  out[3] = aw * bw - ax * bx;
  return out;
}

/**
 * 绕 Y 轴旋转四元数。
 * @param out - 输出的四元数。
 * @param a - 输入的四元数。
 * @param rad - 旋转角度（弧度制）。
 * @returns 返回绕 Y 轴旋转后的四元数。
 */
export function rotateY(out: Quat, a: ReadonlyQuat, rad: number): Quat {
  const halfRad = rad * 0.5;
  const ax = a[0], ay = a[1], az = a[2], aw = a[3];
  const by = Math.sin(halfRad), bw = Math.cos(halfRad);

  out[0] = ax * bw - az * by;
  out[1] = ay * bw + aw * by;
  out[2] = az * bw + ax * by;
  out[3] = aw * bw - ay * by;
  return out;
}

/**
 * 绕 Z 轴旋转四元数。
 * @param out - 输出的四元数。
 * @param a - 输入的四元数。
 * @param rad - 旋转角度（弧度制）。
 * @returns 返回绕 Z 轴旋转后的四元数。
 */
export function rotateZ(out: Quat, a: ReadonlyQuat, rad: number): Quat {
  const halfRad = rad * 0.5;
  const ax = a[0], ay = a[1], az = a[2], aw = a[3];
  const bz = Math.sin(halfRad), bw = Math.cos(halfRad);

  out[0] = ax * bw + ay * bz;
  out[1] = ay * bw - ax * bz;
  out[2] = az * bw + aw * bz;
  out[3] = aw * bw - az * bz;
  return out;
}

/**
 * 根据 X、Y 和 Z 分量计算四元数的 W 分量。
 * @param out - 输出的四元数。
 * @param a - 输入的四元数。
 * @returns 返回计算 W 分量后的四元数。
 */
export function calculateW(out: Quat, a: ReadonlyQuat): Quat {
  const x = a[0], y = a[1], z = a[2];
  out[0] = x;
  out[1] = y;
  out[2] = z;
  out[3] = Math.sqrt(Math.abs(1.0 - x * x - y * y - z * z));
  return out;
}

/**
 * 计算单位四元数的指数。
 * @param out - 输出的四元数。
 * @param a - 输入的四元数。
 * @returns 返回计算指数后的四元数。
 */
export function exp(out: Quat, a: ReadonlyQuat): Quat {
  const x = a[0], y = a[1], z = a[2], w = a[3];
  const r = Math.sqrt(x * x + y * y + z * z);
  const et = Math.exp(w);
  const s = r > 0 ? (et * Math.sin(r)) / r : 0;

  out[0] = x * s;
  out[1] = y * s;
  out[2] = z * s;
  out[3] = et * Math.cos(r);
  return out;
}

/**
 * 计算单位四元数的自然对数。
 * @param out - 输出的四元数。
 * @param a - 输入的四元数。
 * @returns 返回计算自然对数后的四元数。
 */
export function ln(out: Quat, a: ReadonlyQuat): Quat {
  const x = a[0], y = a[1], z = a[2], w = a[3];
  const r = Math.sqrt(x * x + y * y + z * z);
  const t = r > 0 ? Math.atan2(r, w) / r : 0;

  out[0] = x * t;
  out[1] = y * t;
  out[2] = z * t;
  out[3] = 0.5 * Math.log(x * x + y * y + z * z + w * w);
  return out;
}

/**
 * 计算单位四元数的标量幂。
 * @param out - 输出的四元数。
 * @param a - 输入的四元数。
 * @param b - 幂值。
 * @returns 返回计算标量幂后的四元数。
 */
export function pow(out: Quat, a: ReadonlyQuat, b: number): Quat {
  ln(out, a);
  scale(out, out, b);
  exp(out, out);
  return out;
}

/**
 * 在两个四元数之间进行球面线性插值。
 * @param out - 输出的四元数。
 * @param a - 第一个四元数。
 * @param b - 第二个四元数。
 * @param t - 插值因子（范围为 0 到 1）。
 * @returns 返回插值结果四元数。
 */
export function slerp(out: Quat, a: ReadonlyQuat, b: ReadonlyQuat, t: number): Quat {
  let ax = a[0], ay = a[1], az = a[2], aw = a[3];
  let bx = b[0], by = b[1], bz = b[2], bw = b[3];

  let cosom = ax * bx + ay * by + az * bz + aw * bw;
  if (cosom < 0) {
    cosom = -cosom;
    bx = -bx;
    by = -by;
    bz = -bz;
    bw = -bw;
  }

  let scale0, scale1;
  if (1 - cosom > EPSILON) {
    const omega = Math.acos(cosom);
    const sinom = Math.sin(omega);
    scale0 = Math.sin((1 - t) * omega) / sinom;
    scale1 = Math.sin(t * omega) / sinom;
  } else {
    scale0 = 1 - t;
    scale1 = t;
  }

  out[0] = scale0 * ax + scale1 * bx;
  out[1] = scale0 * ay + scale1 * by;
  out[2] = scale0 * az + scale1 * bz;
  out[3] = scale0 * aw + scale1 * bw;
  return out;
}

/**
 * 生成一个随机的单位四元数。
 * @param out - 输出的四元数。
 * @returns 返回随机生成的单位四元数。
 */
export function random(out: Quat): Quat {
  const u1 = Math.random();
  const u2 = Math.random();
  const u3 = Math.random();

  const sqrt1MinusU1 = Math.sqrt(1 - u1);
  const sqrtU1 = Math.sqrt(u1);

  out[0] = sqrt1MinusU1 * Math.sin(2 * Math.PI * u2);
  out[1] = sqrt1MinusU1 * Math.cos(2 * Math.PI * u2);
  out[2] = sqrtU1 * Math.sin(2 * Math.PI * u3);
  out[3] = sqrtU1 * Math.cos(2 * Math.PI * u3);
  return out;
}

/**
 * 计算四元数的逆。
 * @param out - 输出的四元数。
 * @param a - 输入的四元数。
 * @returns 返回计算逆后的四元数。
 */
export function invert(out: Quat, a: ReadonlyQuat): Quat {
  const a0 = a[0], a1 = a[1], a2 = a[2], a3 = a[3];
  const dot = a0 * a0 + a1 * a1 + a2 * a2 + a3 * a3;
  const invDot = dot ? 1.0 / dot : 0;

  out[0] = -a0 * invDot;
  out[1] = -a1 * invDot;
  out[2] = -a2 * invDot;
  out[3] = a3 * invDot;
  return out;
}

/**
 * 计算四元数的共轭。
 * @param out - 输出的四元数。
 * @param a - 输入的四元数。
 * @returns 返回计算共轭后的四元数。
 */
export function conjugate(out: Quat, a: ReadonlyQuat): Quat {
  out[0] = -a[0];
  out[1] = -a[1];
  out[2] = -a[2];
  out[3] = a[3];
  return out;
}

/**
 * 从给定的 3x3 旋转矩阵创建四元数。
 * @param out - 输出的四元数。
 * @param m - 输入的 3x3 旋转矩阵。
 * @returns 返回从矩阵创建的四元数。
 */
export function fromMat3(out: Quat, m: ReadonlyMat3): Quat {
  const fTrace = m[0] + m[4] + m[8];
  let fRoot;

  if (fTrace > 0.0) {
    fRoot = Math.sqrt(fTrace + 1.0);
    out[3] = 0.5 * fRoot;
    fRoot = 0.5 / fRoot;
    out[0] = (m[5] - m[7]) * fRoot;
    out[1] = (m[6] - m[2]) * fRoot;
    out[2] = (m[1] - m[3]) * fRoot;
  } else {
    let i = 0;
    if (m[4] > m[0]) i = 1;
    if (m[8] > m[i * 3 + i]) i = 2;
    const j = (i + 1) % 3;
    const k = (i + 2) % 3;

    fRoot = Math.sqrt(m[i * 3 + i] - m[j * 3 + j] - m[k * 3 + k] + 1.0);
    out[i] = 0.5 * fRoot;
    fRoot = 0.5 / fRoot;
    out[3] = (m[j * 3 + k] - m[k * 3 + j]) * fRoot;
    out[j] = (m[j * 3 + i] + m[i * 3 + j]) * fRoot;
    out[k] = (m[k * 3 + i] + m[i * 3 + k]) * fRoot;
  }

  return out;
}

/**
 * 从给定的欧拉角创建四元数。
 * @param out - 输出的四元数。
 * @param x - 绕 X 轴的旋转角度（度）。
 * @param y - 绕 Y 轴的旋转角度（度）。
 * @param z - 绕 Z 轴的旋转角度（度）。
 * @param order - 旋转顺序，默认为 `'zyx'`。
 * @returns 返回从欧拉角创建的四元数。
 */
export function fromEuler(
  out: Quat,
  x: number,
  y: number,
  z: number,
  order: 'xyz' | 'xzy' | 'yxz' | 'yzx' | 'zxy' | 'zyx' = 'zyx'
): Quat {
  const halfToRad = Math.PI / 360;
  x *= halfToRad;
  y *= halfToRad;
  z *= halfToRad;

  const sx = Math.sin(x), cx = Math.cos(x);
  const sy = Math.sin(y), cy = Math.cos(y);
  const sz = Math.sin(z), cz = Math.cos(z);

  switch (order) {
    case 'xyz':
      out[0] = sx * cy * cz + cx * sy * sz;
      out[1] = cx * sy * cz - sx * cy * sz;
      out[2] = cx * cy * sz + sx * sy * cz;
      out[3] = cx * cy * cz - sx * sy * sz;
      break;
    case 'xzy':
      out[0] = sx * cy * cz - cx * sy * sz;
      out[1] = cx * sy * cz - sx * cy * sz;
      out[2] = cx * cy * sz + sx * sy * cz;
      out[3] = cx * cy * cz + sx * sy * sz;
      break;
    case 'yxz':
      out[0] = sx * cy * cz + cx * sy * sz;
      out[1] = cx * sy * cz - sx * cy * sz;
      out[2] = cx * cy * sz - sx * sy * cz;
      out[3] = cx * cy * cz + sx * sy * sz;
      break;
    case 'yzx':
      out[0] = sx * cy * cz + cx * sy * sz;
      out[1] = cx * sy * cz + sx * cy * sz;
      out[2] = cx * cy * sz - sx * sy * cz;
      out[3] = cx * cy * cz - sx * sy * sz;
      break;
    case 'zxy':
      out[0] = sx * cy * cz - cx * sy * sz;
      out[1] = cx * sy * cz + sx * cy * sz;
      out[2] = cx * cy * sz + sx * sy * cz;
      out[3] = cx * cy * cz - sx * sy * sz;
      break;
    case 'zyx':
      out[0] = sx * cy * cz - cx * sy * sz;
      out[1] = cx * sy * cz + sx * cy * sz;
      out[2] = cx * cy * sz - sx * sy * cz;
      out[3] = cx * cy * cz + sx * sy * sz;
      break;
    default:
      throw new Error(`未知的旋转顺序: ${order}`);
  }

  return out;
}

/**
 * 返回四元数的字符串表示。
 * @param a - 输入的四元数。
 * @returns 返回形如 `quat(x, y, z, w)` 的字符串。
 */
export function str(a: ReadonlyQuat): string {
  return `quat(${a[0]}, ${a[1]}, ${a[2]}, ${a[3]})`;
}

/**
 * 克隆一个四元数。
 * @param a - 输入的四元数。
 * @returns 返回克隆后的四元数。
 */
export function clone(a: ReadonlyQuat): Quat {
  return [a[0], a[1], a[2], a[3]];
}

/**
 * 从值创建一个四元数。
 * @param x - 四元数的 X 分量。
 * @param y - 四元数的 Y 分量。
 * @param z - 四元数的 Z 分量。
 * @param w - 四元数的 W 分量。
 * @returns 返回创建的四元数。
 */
export function fromValues(x: number, y: number, z: number, w: number): Quat {
  return [x, y, z, w];
}

/**
 * 将一个四元数的值复制到另一个四元数。
 * @param out - 输出的四元数。
 * @param a - 输入的四元数。
 * @returns 返回复制后的四元数。
 */
export function copy(out: Quat, a: ReadonlyQuat): Quat {
  out[0] = a[0];
  out[1] = a[1];
  out[2] = a[2];
  out[3] = a[3];
  return out;
}

/**
 * 设置四元数的分量。
 * @param out - 输出的四元数。
 * @param x - X 分量。
 * @param y - Y 分量。
 * @param z - Z 分量。
 * @param w - W 分量。
 * @returns 返回设置分量后的四元数。
 */
export function set(out: Quat, x: number, y: number, z: number, w: number): Quat {
  out[0] = x;
  out[1] = y;
  out[2] = z;
  out[3] = w;
  return out;
}

/**
 * 加两个四元数。
 * @param out - 输出的四元数。
 * @param a - 第一个四元数。
 * @param b - 第二个四元数。
 * @returns 返回两个四元数相加的结果。
 */
export function add(out: Quat, a: ReadonlyQuat, b: ReadonlyQuat): Quat {
  out[0] = a[0] + b[0];
  out[1] = a[1] + b[1];
  out[2] = a[2] + b[2];
  out[3] = a[3] + b[3];
  return out;
}

/**
 * 缩放四元数。
 * @param out - 输出的四元数。
 * @param a - 输入的四元数。
 * @param b - 缩放因子。
 * @returns 返回缩放后的四元数。
 */
export function scale(out: Quat, a: ReadonlyQuat, b: number): Quat {
  out[0] = a[0] * b;
  out[1] = a[1] * b;
  out[2] = a[2] * b;
  out[3] = a[3] * b;
  return out;
}

/**
 * 计算两个四元数的点积。
 * @param a - 第一个四元数。
 * @param b - 第二个四元数。
 * @returns 返回两个四元数的点积值。
 */
export function dot(a: ReadonlyQuat, b: ReadonlyQuat): number {
  return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
}

/**
 * 在两个四元数之间进行线性插值。
 * @param out - 输出的四元数。
 * @param a - 第一个四元数。
 * @param b - 第二个四元数。
 * @param t - 插值因子（范围为 0 到 1）。
 * @returns 返回插值结果四元数。
 */
export function lerp(out: Quat, a: ReadonlyQuat, b: ReadonlyQuat, t: number): Quat {
  const ax = a[0], ay = a[1], az = a[2], aw = a[3];
  const bx = b[0], by = b[1], bz = b[2], bw = b[3];

  out[0] = ax + t * (bx - ax);
  out[1] = ay + t * (by - ay);
  out[2] = az + t * (bz - az);
  out[3] = aw + t * (bw - aw);
  return out;
}

/**
 * 计算四元数的长度。
 * @param a - 输入的四元数。
 * @returns 返回四元数的长度。
 */
export function length(a: ReadonlyQuat): number {
  return Math.sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2] + a[3] * a[3]);
}

/**
 * 长度的别名。
 */
export const len = length;

/**
 * 计算四元数的平方长度。
 * @param a - 输入的四元数。
 * @returns 返回四元数的平方长度。
 */
export function squaredLength(a: ReadonlyQuat): number {
  return a[0] * a[0] + a[1] * a[1] + a[2] * a[2] + a[3] * a[3];
}

/**
 * 平方长度的别名。
 */
export const sqrLen = squaredLength;

/**
 * 归一化四元数。
 * @param out - 输出的四元数。
 * @param a - 输入的四元数。
 * @returns 返回归一化后的四元数。
 */
export function normalize(out: Quat, a: ReadonlyQuat): Quat {
  const len = Math.sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2] + a[3] * a[3]);
  const invLen = len ? 1 / len : 0;

  out[0] = a[0] * invLen;
  out[1] = a[1] * invLen;
  out[2] = a[2] * invLen;
  out[3] = a[3] * invLen;
  return out;
}

/**
 * 检查两个四元数是否完全相等。
 * @param a - 第一个四元数。
 * @param b - 第二个四元数。
 * @returns 如果完全相等返回 `true`，否则返回 `false`。
 */
export function exactEquals(a: ReadonlyQuat, b: ReadonlyQuat): boolean {
  return a[0] === b[0] && a[1] === b[1] && a[2] === b[2] && a[3] === b[3];
}

/**
 * 检查两个四元数是否近似相等。
 * @param a - 第一个四元数。
 * @param b - 第二个四元数。
 * @returns 如果近似相等返回 `true`，否则返回 `false`。
 */
export function equals(a: ReadonlyQuat, b: ReadonlyQuat): boolean {
  return Math.abs(dot(a, b)) >= 1 - EPSILON;
}

/**
 * 使用两个控制点进行球面线性插值。
 * @param out - 输出的四元数。
 * @param a - 第一个四元数。
 * @param b - 第二个四元数。
 * @param c - 第三个四元数。
 * @param d - 第四个四元数。
 * @param t - 插值因子（范围为 0 到 1）。
 * @returns 返回插值结果四元数。
 */
export function sqlerp(out: Quat, a: ReadonlyQuat, b: ReadonlyQuat, c: ReadonlyQuat, d: ReadonlyQuat, t: number): Quat {
  const temp1 = create();
  const temp2 = create();

  slerp(temp1, a, d, t);
  slerp(temp2, b, c, t);
  slerp(out, temp1, temp2, 2 * t * (1 - t));

  return out;
}

/**
 * 根据给定的轴设置四元数。
 * @param out - 输出的四元数。
 * @param view - 视图方向向量。
 * @param right - 右方向向量。
 * @param up - 上方向向量。
 * @returns 返回根据轴设置的四元数。
 */
export function setAxes(out: Quat, view: ReadonlyVec3, right: ReadonlyVec3, up: ReadonlyVec3): Quat {
  const matr: Mat3 = [
    right[0], up[0], -view[0],
    right[1], up[1], -view[1],
    right[2], up[2], -view[2]
  ];
  return normalize(out, fromMat3(out, matr));
}